#ifndef _DRIVETRAIN_H_
#define _DRIVETRAIN_H_

#include "WPILib.h"
#include "../../Robotmap.h"

enum WheelType
{
	FrontLeft = MOTOR_FRONT_LEFT,
	FrontRight = MOTOR_FRONT_RIGHT,
	BackLeft = MOTOR_BACK_LEFT,
	BackRight = MOTOR_BACK_RIGHT
};

class DriveTrain {
	Victor motorFL;
	Victor motorFR;
	Victor motorBL;
	Victor motorBR;

	Encoder encoderFL;
	Encoder encoderFR;
	Encoder encoderBL;
	Encoder encoderBR;

	Gyro gyro;

	Accelerometer accel;

public:
	DriveTrain();
	void DriveDistance(float distance, float time);
	void DriveSpeed(float speed, float time);
	void RotateAngle(float angle, float time);
	void RotateSpeed(float speed, float time);
	void Move(float x, float y, float time);
	
	void SetMotor(WheelType wheel, double speed);
	
	float GetAngle();
	float GetSpeed();
	float GetVelocity();
	float GetAngularVelocity();

};

#endif //_DRIVETRAIN_H_
